﻿#region Using directives
using System;
using Microsoft.Xna.Framework;
using System.Diagnostics;
#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2BroadPhase.h
// Revision   : r142
// Change-Date: 2011-02-03
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2BroadPhase.cpp
// Revision   : r142
// Change-Date: 2011-02-03
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    public struct Pair : IComparable<Pair>, IEquatable<Pair>
    {
        public Int32 proxyIdA;
        public Int32 proxyIdB;
        public Int32 next;

        public int CompareTo(Pair other)
        {
            if (proxyIdA < other.proxyIdA)
            {
                return -1;
            }
            else if (proxyIdA == other.proxyIdA)
            {
                if (proxyIdB < other.proxyIdB)
                {
                    return -1;
                }
                else if (proxyIdB == other.proxyIdB)
                {
                    return 0;
                }
            }

            return 1;
        }

        public static bool operator == (Pair me, Pair other)
        {
            return (me.proxyIdA == other.proxyIdB && me.proxyIdB == other.proxyIdB);
        }

        public static bool operator !=(Pair me, Pair other)
        {
            return (me.proxyIdA != other.proxyIdB || me.proxyIdB != other.proxyIdB);
        }

        public override bool Equals(object obj)
        {
            return this == (Pair)obj;
        }

        public override int GetHashCode()
        {
            return base.GetHashCode();
        }

        #region IEquatable<b2Pair> Member

        public bool Equals(Pair other)
        {
            return (proxyIdA == other.proxyIdA && proxyIdB == other.proxyIdB);
        }

        #endregion
    }

    /// <summary>
    /// The broad-phase is used for computing pairs and performing volume queries and ray casts. 
    /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
    /// It is up to the client to consume the new pairs and to track subsequent overlap.
    /// </summary>
    public class BroadPhase : WorldQueryWrapper
    {
        internal static int e_nullProxy = -1;

        #region Member variables
        DynamicTree m_tree = new DynamicTree();
        Int32 m_proxyCount;
        Int32[] m_moveBuffer;
        Int32 m_moveCapacity;
        Int32 m_moveCount;
        Pair[] m_pairBuffer;
        Int32 m_pairCapacity;
        Int32 m_pairCount;
        Int32 m_queryProxyId;

        #endregion

        public BroadPhase()
        {
            m_proxyCount = 0;

            m_pairCapacity = 16;
            m_pairCount = 0;
            m_pairBuffer = new Pair[m_pairCapacity];

            m_moveCapacity = 16;
            m_moveCount = 0;
            m_moveBuffer = new Int32[m_moveCapacity];
        }

        ~BroadPhase()
        {
            m_moveBuffer = null;
            m_pairBuffer = null;
        }

        ///<summary>
        /// Create a proxy with an initial AABB. Pairs are not reported until UpdatePairs is called.
        ///</summary>
        ///<param name="aabb"></param>
        ///<param name="userData"></param>
        public Int32 CreateProxy(AABB aabb, object userData)
        {
            Int32 proxyId = m_tree.CreateProxy(aabb, userData);
            ++m_proxyCount;
            BufferMove(proxyId);
            return proxyId;
        }

        /// <summary>
        /// Destroy a proxy. It is up to the client to remove any pairs.
        /// </summary>
        /// <param name="proxyId"></param>
        public void DestroyProxy(Int32 proxyId)
        {
            UnBufferMove(proxyId);
            --m_proxyCount;
            m_tree.DestroyProxy(proxyId);
        }

        ///<summary>
        ///Call MoveProxy as many times as you like, then when you are done. 
        ///call UpdatePairs to finalize the proxy pairs (for your time step).
        ///</summary>
        ///<param name="aabb"></param>
        ///<param name="proxyId"></param>
        public void MoveProxy(Int32 proxyId, ref AABB aabb, Vector2 displacement)
        {
            bool buffer = m_tree.MoveProxy(proxyId, ref aabb, displacement);
            if (buffer)
            {
                BufferMove(proxyId);
            }
        }

        /// <summary>
        /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
        /// </summary>
        /// <param name="proxyId"></param>
        public void TouchProxy(int proxyId)
        {
            BufferMove(proxyId);
        }

        /// <summary>
        /// Get the fat AABB for a proxy.
        /// </summary>
        /// <param name="proxyId"></param>
        /// <returns></returns>
        public AABB GetFatAABB(Int32 proxyId)
        {
            return m_tree.GetFatAABB(proxyId);
        }

        /// <summary>
        /// Get user data from a proxy. Returns NULL if the id is invalid.
        /// </summary>
        /// <param name="proxyId"></param>
        /// <returns></returns>
        public Object GetUserData(Int32 proxyId)
        {
            return m_tree.GetUserData(proxyId);
        }

        /// <summary>
        /// Test overlap of fat AABBs.
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <returns></returns>
        public bool TestOverlap(Int32 proxyIdA, Int32 proxyIdB)
        {
            AABB aabbA = m_tree.GetFatAABB(proxyIdA);
            AABB aabbB = m_tree.GetFatAABB(proxyIdB);
            
            //return TestOverlap(aabbA, aabbB);
            // manual inline
            Vector2 d1 = aabbB.lowerBound - aabbA.upperBound;
            Vector2 d2 = aabbA.lowerBound - aabbB.upperBound;

            if (d1.X > 0.0f || d1.Y > 0.0f) return false;
            if (d2.X > 0.0f || d2.Y > 0.0f) return false;

            return true;
        }

        /// <summary>
        /// Get the number of proxies.
        /// </summary>
        /// <returns></returns>
        public Int32 GetProxyCount()
        {
            return m_proxyCount;
        }

        /// <summary>
        /// Update the pairs. This results in pair callbacks. This can only add pairs.
        /// </summary>
        /// <param name="callback"></param>
        public void UpdatePairs(ContactManager callback)
        {
            // Reset pair buffer 
            m_pairCount = 0;

            Int32 i;

            // Perform tree queries for all moving proxies. 
             for (i = 0; i < m_moveCount; ++i) 
             { 
                m_queryProxyId = m_moveBuffer[i]; 
                if (m_queryProxyId == e_nullProxy) 
                { 
                    continue; 
                } 

                // We have to query the tree with the fat AABB so that 
                // we don't fail to create a pair that may touch later. 
                AABB fatAABB = m_tree.GetFatAABB(m_queryProxyId); 

                // Query tree, create pairs and add them pair buffer. 
                m_tree.Query(this, ref fatAABB); 
             } 
      
             // Reset move buffer 
             m_moveCount = 0; 
      
             // Sort the pair buffer to expose duplicates. 
             Array.Sort(m_pairBuffer, 0, m_pairCount);
      
             // Send the pairs back to the client. 
             i = 0; 
             while (i < m_pairCount) 
             { 
                     Pair primaryPair = m_pairBuffer[i]; 
                     Object userDataA = m_tree.GetUserData(primaryPair.proxyIdA); 
                     Object userDataB = m_tree.GetUserData(primaryPair.proxyIdB);

                     callback.AddPair(userDataA, userDataB); 
                     ++i; 
      
                     // Skip any duplicate pairs. 
                     while (i < m_pairCount) 
                     { 
                             Pair pair = m_pairBuffer[i]; 
                             if (pair.proxyIdA != primaryPair.proxyIdA || pair.proxyIdB != primaryPair.proxyIdB) 
                             { 
                                     break; 
                             } 
                             ++i; 
                     } 
             } 
     
             // Try to keep the tree balanced.
             m_tree.Rebalance(4);
        }

        /// <summary>
        /// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB.
        /// </summary>
        /// <param name="callback"></param>
        /// <param name="aabb"></param>
        public void Query(WorldQueryWrapper callback, ref AABB aabb)
        {
            m_tree.Query(callback, ref aabb);
        }

        ///<summary>
        /// Ray-cast against the proxies in the tree. This relies on the callback         
        /// to perform a exact ray-cast in the case were the proxy contains a shape.         
        /// The callback also performs the any collision filtering. This has performance         
        /// roughly equal to k * log(n), where k is the number of collisions and n is the         
        /// number of proxies in the tree.         
        /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
        /// <param name="callback">a callback class that is called for each proxy that is hit by the ray. </param>
        /// </summary>
        internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
        {
            m_tree.RayCast(callback, ref input);
        }

        /// <summary>
        /// Compute the height of the embedded tree.
        /// </summary>
        /// <returns></returns>
        public Int32 ComputeHeight()
        {
            return m_tree.ComputeHeight();
        }

        void BufferMove(Int32 proxyId)
        {
            if (m_moveCount == m_moveCapacity)
            {
                Int32[] oldBuffer = m_moveBuffer;
                m_moveCapacity *= 2;
                m_moveBuffer = new Int32[m_moveCapacity];
                Array.Copy(oldBuffer, m_moveBuffer, m_moveCount);
            }

            m_moveBuffer[m_moveCount] = proxyId;
            ++m_moveCount;
        }

        void UnBufferMove(Int32 proxyId)
        {
            for (Int32 i = 0; i < m_moveCount; ++i)
            {
                if (m_moveBuffer[i] == proxyId)
                {
                    m_moveBuffer[i] = e_nullProxy;
                    return;
                }
            }
        }

        public override bool QueryCallback(Int32 proxyId)
        {
            // A proxy cannot form a pair with itself.
            if (proxyId == m_queryProxyId)
            {
                return true;
            }
            
            // Grow the pair buffer as needed.
            if (m_pairCount == m_pairCapacity)
            {
                Pair[] oldBuffer = m_pairBuffer;
                m_pairCapacity *= 2;
                m_pairBuffer = new Pair[m_pairCapacity];
                Array.Copy(oldBuffer, m_pairBuffer, m_pairCount);
                oldBuffer = null;
            }

            m_pairBuffer[m_pairCount].proxyIdA = System.Math.Min(proxyId, m_queryProxyId);
            m_pairBuffer[m_pairCount].proxyIdB = System.Math.Max(proxyId, m_queryProxyId);
            ++m_pairCount;
            
            return true; 
        }
    }
}